#include  "Can_recive.h"
#include "pid.h"
#include "stdio.h"
#include "math.h"
#include "remote_control.h"
#include "tim.h"
#include "cmsis_os.h"
#include "Seveor_Motor.h"

/*
	圈数设置，后面替换为遥控器数值
	0:初始状态		1：腿在前方	2：腿在后方
*/
float Angle_flag_1=0,Angle_flag_2=0,Angle_flag_3=0,Angle_flag_4=0;

int Chassiss_flag=1;//标志位，检查是否进入任务
/*
    0：电机1；  1：电机2
    2：电机3    3：电机4
*/
pid_Cascade_t motor_pid_Cas[4];//定义串级pid结构体
const Revice_Motro* chuanji_data[4]; //四个电机数据结构


//过零检测数据
float ecd_sum[4],ecd_now[4],ecd_last[4];
static float ecd_err[4];


//1
static float PID_1_Outer[3]={M2006_MOTOR_1_Outer_PID_KP,M2006_MOTOR_1_Outer_PID_KI,M2006_MOTOR_1_Outer_PID_KD};//串级外环PID参数设置,速度环
static float PID_1_Inner[3]={M2006_MOTOR_1_Inner_PID_KP,M2006_MOTOR_1_Inner_PID_KI,M2006_MOTOR_1_Inner_PID_KD};//串级内环PID参数设置，位置环

//2
static float PID_2_Outer[3]={M2006_MOTOR_2_Outer_PID_KP,M2006_MOTOR_2_Outer_PID_KI,M2006_MOTOR_2_Outer_PID_KD};//串级外环PID参数设置,速度环
static float PID_2_Inner[3]={M2006_MOTOR_2_Inner_PID_KP,M2006_MOTOR_2_Inner_PID_KI,M2006_MOTOR_2_Inner_PID_KD};//串级内环PID参数设置，位置环

//3
static float PID_3_Outer[3]={M2006_MOTOR_1_Outer_PID_KP,M2006_MOTOR_3_Outer_PID_KI,M2006_MOTOR_3_Outer_PID_KD};//串级外环PID参数设置,速度环
static float PID_3_Inner[3]={M2006_MOTOR_1_Inner_PID_KP,M2006_MOTOR_3_Inner_PID_KI,M2006_MOTOR_3_Inner_PID_KD};//串级内环PID参数设置，位置环

//4
static float PID_4_Outer[3]={M2006_MOTOR_1_Outer_PID_KP,M2006_MOTOR_4_Outer_PID_KI,M2006_MOTOR_4_Outer_PID_KD};//串级外环PID参数设置,速度环
static float PID_4_Inner[3]={M2006_MOTOR_1_Inner_PID_KP,M2006_MOTOR_4_Inner_PID_KI,M2006_MOTOR_4_Inner_PID_KD};//串级内环PID参数设置，位置环




/*	
		PID初始化     and    获取电机结构体
*/
static void MOTOR_chassis_init()
{
				//1
				PID_init(&motor_pid_Cas[0].inner, PID_POSITION , PID_1_Inner, M2006_MOTOR_1_Inner_PID_MAX_OUT , M2006_MOTOR_1_Inner_PID_MAX_IOUT);
				PID_init(&motor_pid_Cas[0].outer, PID_POSITION , PID_1_Outer ,M2006_MOTOR_1_Outer_PID_MAX_OUT , M2006_MOTOR_1_Outer_PID_MAX_IOUT);
				chuanji_data[0] =	get_Chassis_Motor_Measure_Point(0);

				//2
				PID_init(&motor_pid_Cas[1].inner, PID_POSITION , PID_2_Inner, M2006_MOTOR_2_Inner_PID_MAX_OUT , M2006_MOTOR_2_Inner_PID_MAX_IOUT);
				PID_init(&motor_pid_Cas[1].outer, PID_POSITION , PID_2_Outer ,M2006_MOTOR_2_Outer_PID_MAX_OUT , M2006_MOTOR_2_Outer_PID_MAX_IOUT);
				chuanji_data[1] =	get_Chassis_Motor_Measure_Point(1);

				//3
				PID_init(&motor_pid_Cas[2].inner, PID_POSITION , PID_3_Inner, M2006_MOTOR_3_Inner_PID_MAX_OUT , M2006_MOTOR_3_Inner_PID_MAX_IOUT);
				PID_init(&motor_pid_Cas[2].outer, PID_POSITION , PID_3_Outer ,M2006_MOTOR_3_Outer_PID_MAX_OUT , M2006_MOTOR_3_Outer_PID_MAX_IOUT);
				chuanji_data[2] =	get_Chassis_Motor_Measure_Point(2);

				//4
				PID_init(&motor_pid_Cas[3].inner, PID_POSITION , PID_4_Inner, M2006_MOTOR_4_Inner_PID_MAX_OUT , M2006_MOTOR_4_Inner_PID_MAX_IOUT);
				PID_init(&motor_pid_Cas[3].outer, PID_POSITION , PID_4_Outer ,M2006_MOTOR_4_Outer_PID_MAX_OUT , M2006_MOTOR_4_Outer_PID_MAX_IOUT);
				chuanji_data[3] =	get_Chassis_Motor_Measure_Point(3);
}

/*
		PID计算
*/
void MOTOR_PID_Calculate()
{
			uint8_t i=0;
			static uint8_t InitFlag = 0;
			if(InitFlag == 0)
			{
				for(i=0;i<4;++i)
				{
					ecd_last[i]=chuanji_data[i]->ecd;
				}
				InitFlag = 1;
			}
			else
			{
					for(i=0;i<4;++i)
					{
							ecd_err[i]=chuanji_data[i]->ecd -ecd_last[i];
							ecd_err[i] = ecd_err[i] >  4095 ?   ecd_err[i] - 8191 : ecd_err[i];
							ecd_err[i] = ecd_err[i] < -4095 ?   ecd_err[i] + 8191 : ecd_err[i];
							ecd_sum[i]+=ecd_err[i];
							ecd_last[i]=chuanji_data[i]->ecd;
					}
					PID_calc(&motor_pid_Cas[0].outer,ecd_sum[0],Angle_flag_1*8191/10);//外环计算
					PID_calc(&motor_pid_Cas[0].inner,chuanji_data[0]->rpm, motor_pid_Cas[0].outer.out);//内环计算
					
					PID_calc(&motor_pid_Cas[1].outer,ecd_sum[1],Angle_flag_2*8191/10);//外环计算
					PID_calc(&motor_pid_Cas[1].inner,chuanji_data[1]->rpm, motor_pid_Cas[1].outer.out);//内环计算
				
					PID_calc(&motor_pid_Cas[2].outer,ecd_sum[2],Angle_flag_3*8191/10);//外环计算
					PID_calc(&motor_pid_Cas[2].inner,chuanji_data[2]->rpm, motor_pid_Cas[2].outer.out);//内环计算
				
					PID_calc(&motor_pid_Cas[3].outer,ecd_sum[3],Angle_flag_4*8191/10);//外环计算
					PID_calc(&motor_pid_Cas[3].inner,chuanji_data[3]->rpm, motor_pid_Cas[3].outer.out);//内环计算
			}
			
}



/*
	转圈
	2024.7.12 19:39
*/
void Motor_turn()
{
			//1
		if(Motor_1_angle==1)
		{
				Angle_flag_1=120;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_1_angle==2)
		{
				Angle_flag_1=80;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
				

			//2
		if(Motor_2_angle==1)
		{
				Angle_flag_2=115;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_2_angle==2)
		{
				Angle_flag_2=75;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		
			//3
		if(Motor_3_angle==1)
		{
				Angle_flag_3=90;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_3_angle==2)
		{
				Angle_flag_3=130;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		
			//4
		if(Motor_4_angle==1)
		{
				Angle_flag_4=80;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_4_angle==2)
		{
				Angle_flag_4=120;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
}







/*
	静止
	初始状态
	2024.7.9 13:35
*/
void Mode_static_motor()
{
	Angle_flag_1=45;Angle_flag_2=70;
	Angle_flag_3=95;Angle_flag_4=100;
	MOTOR_PID_Calculate();
	CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
}



/*
		模拟狗的步态，		*								*				*
												*				*								*
												*		->	*				->			*
										*								*				*

		理论存在，实践开始
		2024.7.10	20.43
*/
void Mode__Motor_Forward_LikeDog()
{
			//1
		if(Motor_1_angle==1)
		{
				Angle_flag_1=100;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_1_angle==2)
		{
				Angle_flag_1=0;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
				

			//2
		if(Motor_2_angle==1)
		{
				Angle_flag_2=150;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_2_angle==2)
		{
				Angle_flag_2=100;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		
			//3
		if(Motor_3_angle==1)
		{
				Angle_flag_3=10;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_3_angle==2)
		{
				Angle_flag_3=60;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		
			//4
		if(Motor_4_angle==1)
		{
				Angle_flag_4=60;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_4_angle==2)
		{
				Angle_flag_4=150;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
}



void Mode__Motor_Back_LikeDog()
{
			//1
		if(Motor_1_angle==1)
		{
				Angle_flag_1=30;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_1_angle==2)
		{
				Angle_flag_1=80;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
				

			//2
		if(Motor_2_angle==1)
		{
				Angle_flag_2=75;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_2_angle==2)
		{
				Angle_flag_2=120;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		
			//3
		if(Motor_3_angle==1)
		{
				Angle_flag_3=90;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_3_angle==2)
		{
				Angle_flag_3=40;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		
			//4
		if(Motor_4_angle==1)
		{
				Angle_flag_4=125;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
		else if(Motor_4_angle==2)
		{
				Angle_flag_4=75;
				MOTOR_PID_Calculate();
				CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
		}
}
void Mode_static_motor_LikeDog()
{
	Angle_flag_1=90;
	Angle_flag_2=75;
	Angle_flag_3=90;
	Angle_flag_4=75;
	MOTOR_PID_Calculate();
	CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);	
}

void Feet_Task()
{
	
		MOTOR_chassis_init();
    canfilter_init_start();
	
		Mode_static_motor_LikeDog();
	while(1)
    {
			Chassiss_flag=-Chassiss_flag;//进入任务标志位
			

/****************************调PID时使用********************/
//			MOTOR_PID_Calculate();		
//			CAN_cmd_chassis(motor_pid_Cas[0].inner.out,motor_pid_Cas[1].inner.out,motor_pid_Cas[2].inner.out,motor_pid_Cas[3].inner.out);			

		
			
			if(rc_control->rc.s[1]==1)//前进后退模式
			{
						if(rc_control->rc.ch[3]>600)
						Mode__Motor_Forward_LikeDog();
					else if(rc_control->rc.ch[3]<-600)
						Mode__Motor_Back_LikeDog();
					else
						Mode_static_motor();
			}
			else if(rc_control->rc.s[1]==3)//转弯模式
			{
					if((rc_control->rc.ch[0]<-600)||(rc_control->rc.ch[0]>600))
						Motor_turn();
					else
						Mode_static_motor();
			}
			else if(rc_control->rc.s[1]==2)
				Mode_static_motor();
			
				
			
				

			
				printf("%f,%d,%d\n",motor_pid_Cas[0].outer.out,chuanji_data[0]->ecd,1800);
    }
}

